function [zp,Hv,Hf,Sf]= compute_jacobians(particle, idf, R)

xv= particle.xv;
xf= particle.xf(:,idf);
Pf= particle.Pf(:,:,idf);

for i=1:length(idf)
    % predicted observation
    zp(:,i) = [xf(1,i) - xv(1);
               xf(2,i) - xv(2);
               xf(3,i) - xv(3)];

    % 
    %     R = [cos(yy)*cos(pp)    cos(yy)*sin(pp)*sin(rr)-sin(yy)*cos(rr)     cos(yy)*sin(pp)*cos(rr)+sin(yy)*sin(rr);
    %          sin(yy)*cos(pp)    sin(yy)*sin(pp)*sin(rr)+cos(yy)*cos(rr)     sin(yy)*sin(pp)*cos(rr)-cos(yy)*sin(rr);
    %          -sin(pp)           cos(pp)*sin(rr)                             cos(pp)*cos(rr)                        ];
    % 
    %     r11 = R(1,1);    r12 = R(1,2);    r13 = R(1,3);
    %     r21 = R(2,1);    r22 = R(2,2);    r23 = R(2,3);
    %     r31 = R(3,1);    r32 = R(3,2);    r33 = R(3,3);

    %     Hv(:,:,i) = [ (-r22*r33+r32*r23)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22),  (r12*r33-r32*r13)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22), (-r12*r23+r22*r13)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22),                                                                                             0,                                                                                             0,                                                                                             0;
    %                 (r21*r33-r31*r23)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22), (-r11*r33+r31*r13)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22),  (r11*r23-r21*r13)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22),                                                                                             0,                                                                                             0,                                                                                              0;
    %                 (-r21*r32+r31*r22)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22),  (r11*r32-r31*r12)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22), (-r11*r22+r21*r12)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22),                                                                                             0,                                                                                             0,                                                                                             0];
    % 
    % 
    %     Hf(:,:,i) = [ (r22*r33-r32*r23)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22), (-r12*r33+r32*r13)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22),  (r12*r23-r22*r13)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22);
    %                 (-r21*r33+r31*r23)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22),  (r11*r33-r31*r13)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22), (-r11*r23+r21*r13)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22);
    %                 (r21*r32-r31*r22)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22), (-r11*r32+r31*r12)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22),  (r11*r22-r21*r12)/(-r13*r31*r22+r23*r31*r12-r23*r11*r32+r13*r21*r32-r33*r21*r12+r33*r11*r22)];
    %             

    x0 = xv(1);
    y0 = xv(2);
    z0 = xv(3);
    
    y = pi_to_pi(xv(4)); % roll (x)
    p = pi_to_pi(xv(5)); % pitch (y)
    r = pi_to_pi(xv(6)); % yaw (z)
    cosr = cos(r); sinr = sin(r);
    cosp = cos(p); sinp = sin(p);
    cosy = cos(y); siny = sin(y);

%     xi = zp(1,i);
%     yi = zp(2,i);
%     zi = zp(3,i);

    xi_ = zp(1,i);    yi_ = zp(2,i);    zi_ = zp(3,i);    
    xi = cosy*cosp*xi_+siny*cosp*yi_-sinp*zi_+z0*sinp-cosy*x0*cosp-siny*y0*cosp;
    yi = (siny*cosr-cosy*sinp*sinr)*x0+(cosy*sinp*sinr-siny*cosr)*xi_+(siny*sinp*sinr+cosy*cosr)*yi_+cosp*sinr*zi_-cosp*z0*sinr-cosy*y0*cosr-sinp*y0*siny*sinr;
    zi = (cosy*sinp*cosr+siny*sinr)*xi_+(siny*sinp*cosr-cosy*sinr)*yi_+cosp*cosr*zi_-cosp*z0*cosr+cosy*y0*sinr-sinp*y0*siny*cosr-siny*x0*sinr-sinp*x0*cosy*cosr;
 
    % Jacobian wrt vehicle states
    Hv(:,:,i) = [-cosy*cosp,                -siny*cosp,                  sinp,        -siny*cosp*xi+cosy*cosp*yi+siny*x0*cosp-cosy*y0*cosp,                                                                      -cosy*sinp*xi-siny*sinp*yi-cosp*zi+z0*cosp+cosy*x0*sinp+siny*y0*sinp,                                0;
                 siny*cosr-cosy*sinp*sinr,  -cosy*cosr-siny*sinp*sinr,  -cosp*sinr,  (siny*sinp*sinr+cosy*cosr)*x0+(-cosy*cosr-siny*sinp*sinr)*xi+(cosy*sinp*sinr-siny*cosr)*yi+y0*siny*cosr-y0*cosy*sinp*sinr,  -cosy*cosp*sinr*x0+cosy*cosp*sinr*xi+siny*cosp*sinr*yi-sinp*sinr*zi+sinp*z0*sinr-cosp*y0*siny*sinr,  (-siny*sinr-cosy*sinp*cosr)*x0+(cosy*sinp*cosr+siny*sinr)*xi+(siny*sinp*cosr-cosy*sinr)*yi+cosp*cosr*zi-cosp*z0*cosr+cosy*y0*sinr-sinp*y0*siny*cosr;
                 -siny*sinr-cosy*sinp*cosr, cosy*sinr-siny*sinp*cosr,   -cosp*cosr,  (cosy*sinr-siny*sinp*cosr)*xi+(cosy*sinp*cosr+siny*sinr)*yi-y0*siny*sinr-y0*cosy*sinp*cosr-x0*cosy*sinr+x0*siny*sinp*cosr,  cosy*cosp*cosr*xi+siny*cosp*cosr*yi-sinp*cosr*zi+sinp*z0*cosr-cosp*y0*siny*cosr-cosp*x0*cosy*cosr,   (siny*cosr-cosy*sinp*sinr)*xi+(-cosy*cosr-siny*sinp*sinr)*yi-cosp*sinr*zi+cosp*z0*sinr+cosy*y0*cosr+sinp*y0*siny*sinr-siny*x0*cosr+sinp*x0*cosy*sinr];
 
    % Jacobian wrt feature states
    Hf(:,:,i) = [cosy*cosp,                siny*cosp,                 -sinp;
                 cosy*sinp*sinr-siny*cosr, siny*sinp*sinr+cosy*cosr,  cosp*sinr;
                 cosy*sinp*cosr+siny*sinr, siny*sinp*cosr-cosy*sinr,  cosp*cosr];
    Sf(:,:,i)= Hf(:,:,i) * Pf(:,:,i) * Hf(:,:,i)' + R; % innovation covariance of 'feature observation given the vehicle'
end










